﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using System.Timers;
using System.Windows;
using System.Windows.Input;
using System.Windows.Media;
using RosNode;

namespace RosConsole.Domain
{
    class HomeViewModel: ViewModelBase
    {
        protected WingNode node;
        protected bool Connecting;
        protected Timer timer;
        private readonly Dictionary<bool, Brush> ConnectedColor = new()
        {
            { true, new SolidColorBrush(Colors.MediumSlateBlue) },
            { false, new SolidColorBrush(Colors.LightGray) }
        };
        readonly List<string> ViewProperties = new() { 
            nameof(TeamRole),
            nameof(CtrlMode),
            nameof(RunMode),
            nameof(Battery),
            nameof(x),
            nameof(y),
            nameof(yaw),
            nameof(Lx),
            nameof(Ly),
            nameof(Lyaw),
            nameof(KeepYaw),
            nameof(mps),
            nameof(PidEnable),
            nameof(SetPid),
            nameof(ArmState),
            nameof(RaiseState),
            nameof(RfidState),
            nameof(RfidData),
            nameof(PoseCalibrateX),
            nameof(PoseCalibrateY),
            nameof(PoseCalibrateYaw),
            nameof(PoseCalibrateInfo)
        };
        public HomeViewModel()
        {
            node = WingNode.CreateInstance();
            timer = new(200)
            {
                AutoReset = true,
                Enabled = true
            };
            timer.Elapsed += (object sender, ElapsedEventArgs e) =>
            {
                foreach(var s in ViewProperties)
                {
                    OnPropertyChanged(s);
                }
                var _ = SetPid;
            };
            PublishTestTopic = new AnotherCommandImplementation(_ =>
            {
                node.PublishTestTopic(TestMsg);
            });
            ClearOdom = new AnotherCommandImplementation(_ => node.ClearOdom());
            SetOdom = new AnotherCommandImplementation(_ => OnSetOdom());
            SetKeepYaw_Click = new AnotherCommandImplementation(_ => OnSetKeepYaw());
            AbortMove = new AnotherCommandImplementation(_ => node.AbortMove());
            SetArm = new AnotherCommandImplementation(_ => node.SetArm(SetArmMsg));
            SetRaise = new AnotherCommandImplementation(_ => node.SetRaise(SetRaiseMsg));
            SetRunMode = new AnotherCommandImplementation(_ => node.SetRunMode(SetRunModeValue));
            UpdatePose = new AnotherCommandImplementation(_ => node.UpdatePose());
        }

        public ICommand PublishTestTopic { get; }
        public ICommand ClearOdom { get; }
        public ICommand SetOdom { get; }
        public ICommand SetKeepYaw_Click { get; }
        public ICommand AbortMove { get; }
        public ICommand SetArm { get; }
        public ICommand SetRaise { get; }
        public ICommand SetRunMode { get; }
        public ICommand UpdatePose { get; }

        private Brush _cloudColor = new SolidColorBrush(Colors.LightGray);
        public Brush CloudColor
        {
            get => _cloudColor;
            private set => SetProperty(ref _cloudColor, value);
        }

        public string TeamRole { get => node.TeamRole; }
        public string RunMode { get => node.RunMode;  }
        public string CtrlMode { get => node.CtrlMode; }
        public double x { get => node.driver_state.x; }
        public double y { get => node.driver_state.y; }
        public double yaw { get => node.driver_state.yaw; }
        public double Lx { get => node.driver_state.Lx; }
        public double Ly { get => node.driver_state.Ly; }
        public double Lyaw { get => node.driver_state.Lyaw; }
        public double Battery { get=> node.driver_state.battery; }
        public double KeepYaw { get => node.driver_state.KeepYaw; }
        public bool PidEnable { get => node.driver_state.PidEnable; }
        public double mps { get => node.driver_state.v; }
        public string ArmState { get => node.ArmState; }
        public string RaiseState { get => node.RaiseState; }
        public string RfidState { get => "0x" + node.driver_state.RfidState.ToString("X2"); }
        public string RfidData { get => node.driver_state.RfidData; }
        public double PoseCalibrateX { get => node.pose_calibrate_front.x; }
        public double PoseCalibrateY { get => node.pose_calibrate_front.y; }
        public double PoseCalibrateYaw { get => node.pose_calibrate_front.yaw; }
        public string PoseCalibrateInfo { get => node.pose_calibrate_front.errorinfo; }

        private bool _RosReady = false;
        public bool RosReady
        {
            get => _RosReady;
            set => SetProperty(ref _RosReady, value);
        }

        private bool _SetPid;
        public bool SetPid
        {
            get
            {
                if(_SetPid != node.driver_state.PidEnable)
                {
                    _SetPid = node.driver_state.PidEnable;
                }
                return _SetPid;
            }
            set
            {
                node.PublishTogglePid(value);
                _SetPid = value;
            }
        }

        public string TestMsg { get; set; } = "Testing";
        public string RosIp { get; set; } = Properties.Settings.Default.RosIp;

        private double _OdomX = Properties.Settings.Default.OdomX;
        private double _OdomY = Properties.Settings.Default.OdomY;
        private double _OdomYaw = Properties.Settings.Default.OdomYaw;

        public double OdomX { get => _OdomX; set => SetProperty(ref _OdomX, value); }
        public double OdomY { get => _OdomY; set => SetProperty(ref _OdomY, value); }
        public double OdomYaw { get => _OdomYaw; set => SetProperty(ref _OdomYaw, value); }

        private int _SetKeepYaw = 0;
        public int SetKeepYaw { get => _SetKeepYaw; set => SetProperty(ref _SetKeepYaw, value); }

        private string _SetArmMsg;
        public string SetArmMsg { get => _SetArmMsg; set => SetProperty(ref _SetArmMsg, value); }

        private string _SetRaiseMsg;
        public string SetRaiseMsg { get => _SetRaiseMsg; set => SetProperty(ref _SetRaiseMsg, value); }

        private string _SetRunModeValue;
        public string SetRunModeValue { get => _SetRunModeValue; set => SetProperty(ref _SetRunModeValue, value); }
        public List<string> RunModes { get => node.RunModeDict.Values.ToList(); }

        public void OnConnectRos()
        {
            node.SetRosMasterUri(RosIp);
            if (Connecting)
                return;
            Task.Run(() =>
            {
                Connecting = true;
                node.Init();
                node.InitSubs();
                node.InitPubs();
            }).ContinueWith((task)=> {                
                RosReady = node.NodeReady && node.PubReady && node.SubReady;
                CloudColor = ConnectedColor[RosReady];
                Properties.Settings.Default.RosIp = RosIp;
                Properties.Settings.Default.Save();
                Connecting = false;
            });
        }

        public void OnDisconnect()
        {
            node.DeInit();
            RosReady = node.NodeReady && node.PubReady && node.SubReady;
            CloudColor = ConnectedColor[RosReady];
        }

        public void OnSetOdom()
        {
            Properties.Settings.Default.OdomX = OdomX;
            Properties.Settings.Default.OdomY = OdomY;
            Properties.Settings.Default.OdomYaw = OdomYaw;
            Properties.Settings.Default.Save();
            node.SetOdom(OdomX, OdomY, OdomYaw);
        }

        public void OnSetKeepYaw()
        {
            node.SetYaw(SetKeepYaw);
        }
    }
}
